#!/usr/bin/python

"""
pyode-tutorial-1.py
This is an implementation of the PyODE tutorial 1 program using ode3d.

Written By:
    James Thomas
    Email: jim@houseoftechnology.org
    Web: http://mission-cognition.houseoftechnology.org/

Copyright 2009-2012

This file is part of the PandaODElib distribution.

This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public License
as published by the Free Software Foundation; either version 3 of
the License, or (at your option) any later version. The text of the
GNU Lesser General Public License is included with this library in
the file named LICENSE.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.

Note: This python example follows the tutorial1.py program distributed with
      the PyODE library.
      
      The original PyODE tutorial can be found here:
          http://pyode.sourceforge.net/tutorials/tutorial1.html

For reference here is the original program implemented using PyODE:

# pyODE example 1: Getting started
    
import ode

# Create a world object
world = ode.World()
world.setGravity( (0,-9.81,0) )

# Create a body inside the world
body = ode.Body(world)
M = ode.Mass()
M.setSphere(2500.0, 0.05)
M.mass = 1.0
body.setMass(M)

body.setPosition( (0,2,0) )
body.addForce( (0,200,0) )

# Do the simulation...
total_time = 0.0
dt = 0.04
while total_time<2.0:
    x,y,z = body.getPosition()
    u,v,w = body.getLinearVel()
    print "%1.2fsec: pos=(%6.3f, %6.3f, %6.3f)  vel=(%6.3f, %6.3f, %6.3f)" % \
          (total_time, x, y, z, u,v,w)
    world.step(dt)
    total_time+=dt
"""

from pandac.PandaModules import Mat4

import ode3d
import pvis

# Create a ode3d world object
world = ode3d.World()
world.setGravity((0,-9.81,0))

# Set the pvis scene so we won't have auto-scaling goofing us up.
pvis.base.disableMouse()
pvis.base.camera.setPos(0, 2, 12)
pvis.base.camera.lookAt(0, 1, 0) #@UndefinedVariable -- lookAt(), more Panda3d magic?
pvis.base.camera.setR(180) #@UndefinedVariable -- setR(), more Panda3d magic?

# Turn mouse control back on
mat=Mat4(pvis.base.camera.getMat()) #@UndefinedVariable -- getMat(), more Panda3d magic?
mat.invertInPlace()
pvis.base.mouseInterfaceNode.setMat(mat)
pvis.base.enableMouse()

# Create a ode3d body inside the world
body = ode3d.GDMBody(world)

# Define a physical element (consists of both a physical and pvis representation)
element = ode3d.GDMElement()
element.DefineSphereTotal(1.0, 0.2) # mass, radius -- make it a bit bigger to see

# Add the element to our body, giving the the handle name of 'Sphere'
# Note: A body can have more than one element, making it a compound object
body.AddElement('Sphere', element)

body.setPosition( (0,2,0) )
body.setLinearVel( (2,0,0) ) # Give it some lateral motion for 2D display
body.addForce( (0,200,0) )

# Do the simulation...
framerate = 30
dt = 1.0 / framerate
pvis.rate(framerate) # Try to keep the specified framerate
print 'Close the pvis window to exit'

def loop(task):
    world.step(dt)

    body.UpdateDisplay()
    return task.cont

pvis.Run(loop)
